#!  /usr/bin/env python
# coding=utf-8

import rospy
from pyquaternion import Quaternion
import numpy as np
import json
import matplotlib.pyplot as plt
from ti5arm.msg import ti5_polyJC_Lists

from ti5_fkik import *


from ti5GrabFruits import sub_motorStat
def ti5_getCJdeg():
    """堵塞20ms至收到关节消息/deg,返回的关节角为二维数组
    """
    for i in range(0,10):
        cJdeg = sub_motorStat()
        if  type(cJdeg) != type(-1):
            return cJdeg
    return -1
    pass


# 从dir.txt获取关节角度
def getJdegForm_txt(dir='/home/wheeltec/Desktop/ros_cam/hJ.txt'):
    """从dir.txt获取7个关节角度(deg)，
       返回list
    """
    with open(dir, mode='r') as Jfile:
        Jdata = Jfile.readlines()
        groups_jdeg = (len(Jdata)-1)
        Jdeg = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]*groups_jdeg
        print("groups of Jdeg:",(len(Jdata)-1))
        if(Jdata[0]=="polyJdeg\r\n"):# dir.txt first line must be 'polyJdeg'
            print("open polyJdeg!")
            for i in range(0,groups_jdeg):
                Jdata_split = Jdata[1+i].split(',')
                Jdeg[0+(i*7)] = float(Jdata_split[0])
                Jdeg[1+(i*7)] = float(Jdata_split[1])
                Jdeg[2+(i*7)] = float(Jdata_split[2])
                Jdeg[3+(i*7)] = float(Jdata_split[3])
                Jdeg[4+(i*7)] = float(Jdata_split[4])
                Jdeg[5+(i*7)] = float(Jdata_split[5])
                Jdeg[6+(i*7)] = 0
                pass
        else:
            
            print("first line are not polyJdeg ",Jdata[0])
            pass
        pass# with

    return Jdeg
    pass

from interPolyPointPub import move_j
def move_handeye_path(dir):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0
 
    # 需要运行第几步就给 pose_step赋几
    pose_step = 0 # 默认不运行
    
    # 若使用实时读取的位置，令use_realtimePose=1，这可能会花费更多的运行时间
    use_realtimePose = 1
 
    pub = rospy.Publisher("interPolyPoint_topic",ti5_polyJC_Lists,queue_size=1000)
 
    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start_move+++*********")

    sJDeg = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    eJDeg = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    #jointxRadList is deg
    nJ = getJdegForm_txt(dir)
    max_step = len(nJ)/7
    nJ = np.resize(nJ,(len(nJ)/7,7))
    print("共{}组位置".format(max_step))
    # run path 1
    while True:
        print("输入'q'结束移动并退出，输入'n'移动到下一位置 (输入字符串需要包括引号)")
        cmd_in = input("请输入选项：")
        if cmd_in == "q":
            print("正在退出...")
            return
        elif cmd_in == "n":
            pose_step=pose_step+1
            pass

        if(pose_step>0 and pose_step<=max_step):

            eJDeg = nJ[pose_step-1,0:6]

            if (use_realtimePose==1):
                arm_cjDeg = ti5_getCJdeg()
                if type(arm_cjDeg) != type(-1):
                    sJDeg = arm_cjDeg[0,0:6]
                else:
                    print("get current JointDeg failed,please retry!")
                    return
            else:
                if (pose_step>1):
                    sJDeg = nJ[pose_step-2,0:6]
                    pass

            move_j(sJDeg = sJDeg,
                    eJDeg = eJDeg,
                    pub=pub,
                    cmd_rate=250,
                    use_time=6.0)

        
            print("pose_step=",pose_step)
            print("ok")
            rate.sleep()
            if pause_more:
                rate.sleep()
                rate.sleep()
        else:
            print("pose_step超过最大步数,正在退出...")
            return
    pass



if  __name__== "__main__":
    rospy.init_node("hand_eye_move_node")


    move_handeye_path(dir="/home/wheeltec/Desktop/ros_cam/hJ.txt")

    pass



